//
// Created by daybeha on 23-11-27.
//
#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"

using namespace std;

string traj_outpath;


unsigned int cnt=0;

double pre_time;
Eigen::Vector3d pre_acc;
Eigen::Vector3d pre_gyro;

Eigen::Vector3d gw;          //  重力加速度
Eigen::Vector3d gw_sum;

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    cnt++;
    // 处理IMU数据，进行必要的计算
    Eigen::Vector3d curr_acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z);
    Eigen::Vector3d curr_gyro(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
    Eigen::Quaterniond Qwb = Eigen::Quaterniond(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z);
//        cout << "curr_acc: " << curr_acc(0) << " " << curr_acc(1) << " " << curr_acc(2) << endl;
//        cout << "curr_gyro: " << curr_gyro(0) << " " << curr_gyro(1) << " " << curr_gyro(2) << endl;
//        cout << "Qwb: " << Qwb.w() << " " << Qwb.x() << " " << Qwb.y() << " " << Qwb.z() << endl;


    Eigen::Vector3d curr_gw = Qwb.normalized() * curr_acc;
    if (cnt < 101){
        gw_sum += curr_gw;
    } else if((curr_gw - gw).norm() < 0.025){
        gw_sum += curr_gw;
    } else{
        cnt--;
        cerr << "gravity measurement error, leave imu alone!" << endl;
        cout << "curr_gw - gw: " << (curr_gw - gw).norm() << endl;
    }

    if(cnt%100==0){
        gw = gw_sum / cnt;
        cout << cnt << "/ gw: " << gw(0) << ", " << gw(1) << ", " << gw(2) << endl;
    }
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "imu_gravity");
    ros::NodeHandle node("~");
//    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);

    int traj_format; // 0: kitti; 1:tum
    node.param<int>("/traj_format", traj_format, 1);
    node.param<string>("/traj_outpath", traj_outpath,
                       "/home/daybeha/Documents/github/vins_ws/src/VINS-Fusion/results/");

    string imu_topic;
    node.param<string>("/imu_topic", imu_topic,
                       "/imu/data_raw");


    ros::Subscriber sub_imu=node.subscribe(imu_topic, 2000, imu_callback, ros::TransportHints().tcpNoDelay());

    return 0;
}
